#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <ServoSoft.h>
#include <MsTimer2.h>
#include <defArduinoParaArduino.h>

#define PULSES_PER_MM 130/499
#define PULSES_PER_DEG 52/90

#define IR_FD_PIN 3 //InfraRed Frontal Direito no ananolg 3
#define IR_FE_PIN 4
#define IR_TD_PIN 5
#define IR_TE_PIN 6
#define IR_MD_PIN 7
#define IR_MD_PIN 2

#define bitG 4 //digital 4
#define bitR 5
#define bitD 7
#define bitS 8

#define MOTOR_A 'a'
#define MOTOR_B 'b'
#define MOTOR_C 'c'
#define MOTOR_D 'd'

// Ma - Frente - Esquerda
#define Ma_b1 14
#define Ma_b2 15

// Mb - Tras - Direita
#define Mb_b1 5 
#define Mb_b2 4 

// Mc - Tras - Esquerda
#define Mc_b1 7 
#define Mc_b2 8 

// Md - Frente - Direita
#define Md_b1 13
#define Md_b2 16

#define ENABLE 6

 
char msg[16];
char msgSize;
char response[16];
int numSeqEsperado = 1;

volatile long int pulsosLidos = 0;
volatile int flag = LOW;


 Servo servo01; // traseira - esquerda
 Servo servo02; // traseira - direita
 Servo servo03; // frontal - direita 
 Servo servo04; // frontal - esquerda 
 
void refreshServos()
{
  Servo::refresh();
}


int serialGetMsg()
{
  int i;  
  i=0;
  
    while (Serial.available())
        msg[i++] = Serial.read();
    
  if(i>0)
  {
    msg[i]='\0';
    Serial.flush();
  }
  
  msgSize=i;
  
  return i;
}


void sendMsg( int mensagem )
{
  char resposta[8],aux[4];
  aux[0]='\0';
  
  resposta[0] = '<';
  resposta[1] = '\0';

  sprintf(aux,"%d",mensagem); // transforma o int em string
    
  if ( strlen(aux) == 1 )
  {
    strcpy(resposta,"<000");
    strcat(resposta,aux);
  
  } else 
      if ( strlen(aux) == 2 )
      {
         strcpy(resposta,"<00");
         strcat(resposta,aux);
      }
      else 
        if( strlen(aux) == 3 )
        {
          strcpy(resposta,"<0");
          strcat(resposta,aux);
        }
        else
          strcat(resposta,aux);
        
 resposta[5] = '>';
 resposta[6] = '\0';

  Serial.println(resposta);
  
}


void avante()
{
    servo01.write(90);
    servo02.write(0);
    servo03.write(90);
    servo04.write(0);
    Servo::refresh();

    delay(700);    
}


void aoLado()
{
    servo01.write(0);
    servo02.write(90);
    servo03.write(0);
    servo04.write(90);
    Servo::refresh();    

    delay(700);
}


void girar()
{
    servo01.write(45);
    servo02.write(45);
    servo03.write(45);
    servo04.write(45);
    Servo::refresh();    
    
    delay(500);
}


void setMotor(char Id, int estado)
{
  short int b1=0,b2=0;
  
  switch( Id )
  {
    case MOTOR_A: b1 = Ma_b1;
                  b2 = Ma_b2;
    break;
    
    case MOTOR_B: b1 = Mb_b1;
                  b2 = Mb_b2;
    break;
    
    case MOTOR_C: b1 = Mc_b1;
                  b2 = Mc_b2;
    break;
    
    case MOTOR_D: b1 = Md_b1;
                  b2 = Md_b2;
    break;
  }
  
  switch( estado )
  {
    case FWD:   digitalWrite(b1,LOW);
                digitalWrite(b2,HIGH);
    break;
    
    case REV:  digitalWrite(b1,HIGH);
               digitalWrite(b2,LOW);
    break;
    
    case OFF:  digitalWrite(b1,LOW);
               digitalWrite(b2,LOW);
    break;  
  
  }

}


void stop()
{
  setMotor(MOTOR_A,OFF);
  setMotor(MOTOR_B,OFF);
  setMotor(MOTOR_C,OFF);
  setMotor(MOTOR_D,OFF);
  
  delay(300);
}


void go_rev(long int rotacoes)
{
  avante();
  
  //interrupts();
  pulsosLidos = 0;
  
  setMotor(MOTOR_A,REV);
  setMotor(MOTOR_B,REV);
  setMotor(MOTOR_C,REV);
  setMotor(MOTOR_D,REV);
  
  while( pulsosLidos < rotacoes )
  { delay(5); }
  
  stop();
  
  //noInterrupts();
  
  pulsosLidos = 0;
  
  //Serial.println("OK");
}


void go_fwd(long int rotacoes)
{
  avante();
  
  //interrupts();
  pulsosLidos = 0;
  
  setMotor(MOTOR_A,FWD);
  setMotor(MOTOR_B,FWD);
  setMotor(MOTOR_C,FWD);
  setMotor(MOTOR_D,FWD);
  
  //while( pulsosLidos < rotacoes )
  //{ delay(5); }
  
  delay(1000);
  
  stop();
  
  //noInterrupts();
  pulsosLidos = 0;
  
  //Serial.println("OK");
}


void go_left(long int rotacoes)
{
  aoLado();
 
  //interrupts();
  pulsosLidos = 0;
  
  setMotor(MOTOR_A,REV);
  setMotor(MOTOR_B,REV);
  setMotor(MOTOR_C,FWD);
  setMotor(MOTOR_D,FWD);
  
  while( pulsosLidos < rotacoes )
  { delay(5); }
  
  stop();
  
  //noInterrupts();
  pulsosLidos = 0;
  
  //Serial.println("OK");
}


void go_right(long int rotacoes)
{
  
  aoLado();
 
  //interrupts();
  pulsosLidos = 0;
  
  setMotor(MOTOR_A,FWD);
  setMotor(MOTOR_B,FWD);
  setMotor(MOTOR_C,REV);
  setMotor(MOTOR_D,REV);
  
  while( pulsosLidos < rotacoes )
  { delay(5); }
  
  stop();
  
  //noInterrupts();
  pulsosLidos = 0;
  
  //Serial.println("OK");
}


void turn_ccw(long int rotacoes)
{
  girar();
  
  //interrupts();
  pulsosLidos = 0;
   
  setMotor(MOTOR_A,REV);
  setMotor(MOTOR_B,FWD);
  setMotor(MOTOR_C,REV);
  setMotor(MOTOR_D,FWD);
  
  while( pulsosLidos < rotacoes )
  { delay(5); }
  
  stop();
  
  //noInterrupts();
  pulsosLidos = 0;
  
  //Serial.println("OK");
}


void turn_cw(long int rotacoes)
{
  girar();
  
  //interrupts();
  pulsosLidos = 0;
  
  setMotor(MOTOR_A,FWD);
  setMotor(MOTOR_B,REV);
  setMotor(MOTOR_C,FWD);
  setMotor(MOTOR_D,REV);
  
  while( pulsosLidos < rotacoes )
  { delay(5); }
  
  stop();
  
  //noInterrupts();
  pulsosLidos = 0;
  
  //Serial.println("OK");  
}


void girarSobre( int qualServo, int interno )
{
  short int S1,S2;
  
  
  if( interno )
  {
    S1 = REV;
    S2 = FWD;
  }
  else          
  {
    S1 = FWD;
    S2 = REV;
  }
            
  
  switch( qualServo )
  {
    case 1: servo02.write(0);
            servo03.write(45);
            servo04.write(90);
            Servo::refresh();
            delay(200);
            
            setMotor(MOTOR_A,S1);
            setMotor(MOTOR_B,S2);
            setMotor(MOTOR_C,OFF);
            setMotor(MOTOR_D,S2);

    break;
    
    case 2: servo01.write(90);
            servo03.write(0);
            servo04.write(45);
            Servo::refresh();
            delay(200);
            
            setMotor(MOTOR_A,S2);
            setMotor(MOTOR_B,OFF);
            setMotor(MOTOR_C,S2);
            setMotor(MOTOR_D,S1);

    break;  
    
    case 3: servo01.write(45);
            servo02.write(90);
            servo04.write(0);
            Servo::refresh();
            delay(400);
              
            setMotor(MOTOR_A,S1);
            setMotor(MOTOR_B,S2);
            setMotor(MOTOR_C,S1);
            setMotor(MOTOR_D,OFF);
  
 
    break;
    
    case 4: servo01.write(0);
            servo02.write(45);
            servo03.write(90);
            Servo::refresh();
            delay(300);
            
            setMotor(MOTOR_A,OFF);
            setMotor(MOTOR_B,S1);
            setMotor(MOTOR_C,S2);
            setMotor(MOTOR_D,S1);

    break;
  
  }

}



void alinharIR(int face, int intOUext )
{
  short int IR1,TH1,IR2,TH2,PG1,PG2,IR1_val,IR2_val,S1,S2,faceFita; //,ajuste,flag=1;
 
  analogWrite(ENABLE,55);
  
  
  if( intOUext == INTERNO )
  {
    S1 = REV;
    S2 = FWD;
  }
  else
  {
    S1 = FWD;
    S2 = REV;
  } 

DENOVO:

    switch( face )
    {
      case FRENTE: IR1 = IR_FD_PIN;
                   TH1 = THRESHOLD_IR_FD; 
                   PG1 = 3; // Ponto de Giro sobre o servo3 
                   IR2 = IR_FE_PIN;
                   TH2 = THRESHOLD_IR_FE;
                   PG2 = 4;
                   faceFita = intOUext;
                   //ajuste = ( interno ? FRENTE : TRAS );  
                   
                   avante();  
                   setMotor(MOTOR_A,S1);
                   setMotor(MOTOR_B,S1);
                   setMotor(MOTOR_C,S1);
                   setMotor(MOTOR_D,S1);
                   
      break;
     
      case TRAS:   IR1 = IR_TD_PIN;
                   TH1 = THRESHOLD_IR_TD;
                   PG1 = 2;
                   IR2 = IR_TE_PIN;
                   TH2 = THRESHOLD_IR_TE;
                   PG2 = 1;
                   faceFita = intOUext;
                   //ajuste = ( interno ? TRAS : FRENTE );  
                   
                   avante();
                   setMotor(MOTOR_A,S2);
                   setMotor(MOTOR_B,S2);
                   setMotor(MOTOR_C,S2);
                   setMotor(MOTOR_D,S2);
      break;
      
      case LAT_ESQ: IR1 = IR_FE_PIN;
                    TH1 = THRESHOLD_IR_FE;  
                    PG1 = 4;
                    IR2 = IR_TE_PIN;
                    TH2 = THRESHOLD_IR_TE;  
                    PG2 = 1;
                    faceFita = !intOUext;
                    //ajuste = ( interno ? LAT_ESQ : LAT_DIR );
                    
                    aoLado();
                    setMotor(MOTOR_A,S2);
                    setMotor(MOTOR_B,S2);
                    setMotor(MOTOR_C,S1);
                    setMotor(MOTOR_D,S1);
  
      break;
      
      case LAT_DIR: IR1 = IR_FD_PIN;
                    TH1 = THRESHOLD_IR_FD;  
                    PG1 = 3;
                    IR2 = IR_TD_PIN;
                    TH2 = THRESHOLD_IR_TD;
                    PG2 = 2;
                    faceFita = !intOUext;
                    //ajuste = ( interno ? LAT_DIR : LAT_ESQ );
                   
                    aoLado();
                    setMotor(MOTOR_A,S1);
                    setMotor(MOTOR_B,S1);
                    setMotor(MOTOR_C,S2);
                    setMotor(MOTOR_D,S2);
      break;
    
    }
    
    while( ( (IR1_val = analogRead(IR1)) < TH1 ) && ( (IR2_val = analogRead(IR2)) < TH2 ) )
      delay(3);
    
    stop();
    
   while( ( (IR1_val = analogRead(IR1)) < TH1 ) || ( (IR2_val = analogRead(IR2)) < TH2 ) )
   {
      if( IR1_val > TH1 )
      {
        if( analogRead(IR2) < TH2  )
          girarSobre( PG1, faceFita );
      
        while( analogRead(IR2) < TH2  )
        { delay(5); }
        
        
        delay(50); // VEEEEEEEEEEEEEERRRRRRRR ISSO!
        stop();      
      }
      else if( IR2_val > TH2 )
           { 
              if( analogRead(IR1) < TH1  )
                girarSobre( PG2, faceFita );
              
              while( analogRead(IR1) < TH1  )
              { delay(5); }
         
              delay(50); // VEEEEEEEEEEEEEERRRRRRRR ISSO!    
              stop();  
           }
           else
            goto DENOVO; // dois sensores fora da fita denovo (improvavel, mas tem q considerar..)
  
   }
   
   
   stop();
   
   //Serial.println("OK");
 
   if( (face == FRENTE) || (face == TRAS) )
     avante();
   else
     aoLado();  

   digitalWrite(ENABLE,HIGH); 
}
 


void leSensores()
{

  char sensorId[4];
  int sensorID;
  
  sensorId[0] = msg[1];
  sensorId[1] = msg[2];
  sensorId[2] = msg[3];
  sensorId[3] = '\0';
  
  sensorID = atoi(sensorId);
  
  int leitura = sensorID;

  switch( sensorID )
  {
    case IR_FD: leitura = analogRead(IR_FD_PIN);
                break;
    
    case IR_FE: leitura = analogRead(IR_FE_PIN);
                break;
    
    case IR_TD: leitura = analogRead(IR_TD_PIN);
                break;
    
    case IR_TE: leitura = analogRead(IR_TE_PIN);
                break;
    
  }

  sendMsg( leitura );
  
}



int pegaInstrucao()
{

  long int arg = 0,pulses_mm,pulses_deg;  

  arg = (msg[1] - '0')*100;
  arg = arg + (msg[2] - '0')*10;
  arg = arg + (msg[3] - '0')*1;

  if( arg < 0 || arg > 999 ) // soh pra garantir
    return -1;

  pulses_mm = arg*(float)PULSES_PER_MM; // mm -> pulsos
  
  pulses_deg = arg*(float)PULSES_PER_DEG; // degrees -> pulsos
  
      switch( msg[4] )
      {
        case OMNI_FWD: go_fwd(pulses_mm);
                       break;
        
        case OMNI_REV: go_rev(pulses_mm);
                       break;
                      
        case OMNI_DIR: go_right(pulses_mm);
                       break;
                  
        case OMNI_ESQ: go_left(pulses_mm);
                       break;
                                    
        case OMNI_CCW: turn_ccw(pulses_deg);
                       break;
                  
       case OMNI_CW: turn_cw(pulses_deg);
                     break;

       case OMNI_ALINHA_IR: alinharIR(msg[2]-'0',msg[3]-'0'); // transforma em int
                            break;
       
       case LE_SENSORES: leSensores();
                         break; 

       default : return -1;
      
      }

return 0;
}

void encoder()
{
     pulsosLidos++;
}


void setup() 
{ 
  
  MsTimer2::set(20,refreshServos);
  Serial.begin(115200);
  
  attachInterrupt(0, encoder, RISING);
  
  pinMode(ENABLE,OUTPUT);
  
  pinMode(Ma_b1,OUTPUT);
  pinMode(Ma_b2,OUTPUT);
  
  pinMode(Mb_b1,OUTPUT);
  pinMode(Mb_b2,OUTPUT);
  
  pinMode(Mc_b1,OUTPUT);
  pinMode(Mc_b2,OUTPUT);
  
  pinMode(Md_b1,OUTPUT);
  pinMode(Md_b2,OUTPUT);
  
  
  digitalWrite(ENABLE,HIGH);
  

/* --- Configuracao para os servos MG946R */

  servo01.attach(9); 
  servo01.setMinimumPulse(840);
  servo01.setMaximumPulse(2680);

  servo02.attach(10); 
  servo02.setMinimumPulse(800);
  servo02.setMaximumPulse(2680);

  servo03.attach(11); 
  servo03.setMinimumPulse(900);
  servo03.setMaximumPulse(2830);
  
  servo04.attach(12); 
  servo04.setMinimumPulse(880);
  servo04.setMaximumPulse(2830);
    
  MsTimer2::start();
  
  avante();
} 



void loop() 
{  
  int n=0;
  int numSeqRecebido = -1;
//  while(1)
  //  alinharIR(FRENTE,INTERNO); */
   
  delay(5);// wait for serial input
    
  n = serialGetMsg();
  
  if( n )
  {
    if( (msg[0]=='<') && (msg[6]=='>') && (strlen(msg) == 7) )
    {
      numSeqRecebido = (msg[5]-'0'); 
          
            if( numSeqRecebido == numSeqEsperado )
            {
              if( pegaInstrucao( ) == 0 ) // executa
              {
                 Serial.println("ACK");
                 
                   if( numSeqEsperado == 9)
                     numSeqEsperado = 1;
                   else
                     numSeqEsperado++;
              }
              else
                Serial.println("NACK");
            }
            else  
              Serial.println("NACK");  
     }
     else if( strcmp(msg,"[START]") == 0 )
          { 
            numSeqEsperado = 1;
            Serial.println("ACK");
          }
          else
            Serial.println("NACK");
    
  }
    
} 
